#include "stepper.h"
#include "usart.h"

#define motor motors[0]

AxisData motors[] = {
  {
    .status=0X0010,
    .position = 0x800000,
    .speed = 1,
    .microstep = 0x00,
    .pwmindex = MICROSTEPS - 1,
    .breaks = 400,
    .phase = STOP,
    .timer= {
      .count=138,
      .current=0,
      .timer={ .value= 65535 - 65217 }
    },
    .tfinal = 0,
    .tinitial = 0,
    .deltat = 0,
    .ta = 2000,
    .id = '1'
  }
};

// linear acceleration table
unsigned char tspeed[] = { 17,240,242,243,244,245,246,247,247,248,248,249,249,249,250,250,250,250,250,251,251,251,251,251,251,252,252,252,252,252,252,252,252,252,252,252,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,253,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,254,255,255,255,255,255};

/*
void init_pwm(void) {
  PTCON0 = 0x00; // postscaler = 1:1 (0000); prescaler= 1:1 (00); Free running mode (00)
  PTCON1 = 0x00; // PTMR off (0); PTMR direction upwards (0); unused (000000)
  // PWMCON0 = 0x6F; // unused (0); pwm1&3 enabled (0110); all independent pairs (1111)
  PWMCON0 = 0x7F; // unused (0); pwm1&3&5&7 enabled (0111); all independent pairs (1111)
  PWMCON1 = 0x00; //??
  //    PTPERL = 0xff; // Fpwm = FOSC / (4 * (PTPER + 1))= 19531.25 Hz
  //    PTPERL = 0x3f; // Fpwm = 78125Hz@20Mhz 8bits resolution, 156250Hz@40Mhz
  PTPERL = 0x7f; // Fpwm = 78125Hz@40Mhz 9bits resolution
  //    PTPERL = 0x1f; // Fpwm = 156250Hz
  PTPERH = 0x00;

  //Start PWM
  PTCON1bits.PTEN=1;
}
*/
void init_timers(void) {
  // Init TMR1
  T1CON = 0x04; // 2x8bits mode (RD16=0), TMR1RUN is read only (0), prescale = 1 (00), 
                // use internal clock (000), don't run yet (TMR1ON = 0)  
}


/* Motor Start/Stop */

unsigned long get_timer_reg(unsigned long v) {
  unsigned long tc = (360 * MICROSEC) / STEPS360;
  return ((tc * 1000) / (v * TPNS));
}

void compute_timer() {
  // vfinal = motor.speed, vinitial = current speed 
  unsigned int vinitial;
  if (!(GETMOTORPROPERTY(motor.status, RUNNING)))
    motor.treg = get_timer_reg(1);
  vinitial = (unsigned int) get_timer_reg(motor.treg);
  motor.timerfinal=get_timer_reg(motor.speed);
  motor.totalreg = (vinitial*(MICROSEC / TPNS))/ ((motor.speed - vinitial));
  motor.totalreg = motor.totalreg * motor.ta;
  motor.constreg=(((NANOSEC / TPNS)) / (STEPS360));
  motor.constreg = 360*motor.constreg;
  motor.constreg= motor.constreg / ((motor.speed -vinitial) * TPNS); 
  if (motor.speed > vinitial) {
    LED_YELLOW=0;
    LED_GREEN=1;
    motor.phase=ACCEL;
  } else {
    LED_YELLOW=1;
    LED_GREEN=0;
    /*    motor.totalreg = (motor.speed*(MICROSEC / TPNS))/ ((vinitial - motor.speed));
    motor.totalreg = motor.totalreg * motor.ta;
    motor.constreg=(((NANOSEC / TPNS)) / (STEPS360));
    motor.constreg = 360*motor.constreg;
    motor.constreg= motor.constreg / ((vinitial - motor.speed) * TPNS); 
    */
    if (motor.phase == STOPPING) motor.phase=DECELSTOP; else motor.phase=DECEL;
  }
  SETTIMER;
}

void resume() {
  compute_timer();
  SETMOTORPROPERTY(motor.status, RUNNING);
  STARTTIMER;
}


void pause() {
  motor.speed=1;
  motor.phase=STOPPING;
  //compute_timer();
  //UNSETMOTORPROPERTY(motor.status, RUNNING);
}

void stop() {
  STOPTIMER;
  UNSETMOTORPROPERTY(motor.status, RUNNING);
}


/*** Interrupts ***/
/** High Priority Interrupt Subroutine **/
void high_isr (void) __interrupt 1 {
  unsigned char step_inc;
  unsigned char axisindex=0;
  if (PIR1bits.TMR1IF==1)  {
    RESETTIMER;
    motor.timer.current+=1;
    if (motor.timer.current == motor.timer.count) {
      motor.timer.current=0;
      if ((motor.phase==SPEEDCHANGE) || (motor.phase == STOPPING)) compute_timer();
      if ((motor.phase == DECELSTOP) && (motor.treg >= motor.timerfinal)) { motor.phase = STOP; STOPTIMER; UNSETMOTORPROPERTY(motor.status, RUNNING);LED_YELLOW=1; LED_GREEN=1;}
      if (((motor.phase == ACCEL) && (motor.treg <= motor.timerfinal)) || ((motor.phase == DECEL) && (motor.treg >= motor.timerfinal))) {
	motor.phase = RUN;
	LED_YELLOW=0; LED_GREEN=0;
      }
      if (((motor.phase == ACCEL) && (motor.treg > motor.timerfinal)) || ((motor.phase == DECEL) && (motor.treg < motor.timerfinal))) {
	signed long temp;
	motor.totalreg += motor.treg;
	temp = (motor.ta * MICROSEC) / motor.totalreg;
	motor.treg= (motor.constreg * temp);
	SETTIMER;
	if (motor.phase== ACCEL) LED_YELLOW=!LED_YELLOW; 
	if (motor.phase== DECEL) LED_GREEN=!LED_GREEN; 
      }
      step_inc=1; 
      if (GETMOTORPROPERTY(motor.status, BACKWARD)) {
	motor.position -= step_inc; motor.microstep -= step_inc; 
      } else {
	motor.position += step_inc; motor.microstep += step_inc; 
      }
      motor.microstep &= (4 * MICROSTEPS) - 1;
      SETPHIA(motor.microstep);
      SETPHIB(motor.microstep);
      PIR1bits.TMR1IF = 0;
    }
  }
}

/** Low Priority Interrupt Subroutine **/
void low_isr (void) __interrupt 2 { // low priority interrupt

   if (PIR1bits.RCIF) {   // Usart Receive
     unsigned char c;
     c=RCREG;
     cmd[usart_index]=c;
     usart_index+=1;
     if (usart_index==32) { send_byte(0x01); usart_index=0;}
     if (c == 0x2e) usart_index=0; // flush '.' ending commands
     if (c == 0x0D) cmd[usart_index - 1] = (c = 0);
     if (c==0) usart_rdy=1;
  }
} 

void main() {

  /** Init interrupts **/
  /* Using priority */
  RCONbits.IPEN=1;
  /* Interrupt priority mask */
  // high priority interrupts
  IPR1bits.TMR1IP=1;
  // low priority interrupts
  IPR1bits.RCIP=0;
  /* Interrupt Mask */ 
  PIE1bits.TMR1IE=1;
  
  INTCONbits.GIE=1; // enable high priority ints
  INTCONbits.PEIE=1; // enable low priority ints
  
  /** Init I/O Ports **/

  SETMOTOROUTPUTPORTS;
  ENA=0;
  ENB=0;
  SETLEDOUTPUTPORTS;
  LED_YELLOW=1; LED_GREEN=1;
  //SETINPUTPORTS;
  
  /** Init Usart **/
  usart_open(129); /* 10-> 115200, 32 -> 38400, 64 -> 19200 , 129 -> 9600 @20Mhz */

  while (1) {
    if (usart_rdy) {            // Usart cmd received
      if (cmd[0] != ':') { send_byte('!'); goto next_cmd;}
      switch (cmd[1]) {
	/** Simple motor Protocol **/
      case 'v': 				// Get firmware version
        send_byte('='); send_string(VERSION);
	break;
      case 'f':				// Get motor status
	send_byte('=');send_u12(motor.status);send_u8(motor.phase);
	break;
      case 'j': 		               // Get encoder values
	send_byte('=');send_u24(motor.position);
	break;
      case 'g':				// get high speed ratio
	send_byte('=');send_u24(0x000001);
	break;
      case 'E':                                          	// Set encoder values
	motor.position=get_u24();
	send_byte('=');
	break;
      case 'F':                                          	// Initialize & activate motors
	//init_pwm();	
	init_timers();
	SETPHIA(motor.microstep); SETPHIB(motor.microstep);
	SETMOTORPROPERTY(motor.status, INITIALIZED);
	ENA = 1;
	ENB = 1;
	send_byte('=');
	break;

      case 'J':						// Start motor
	resume();
	send_byte('=');
	break;
      case 'K':					// Stop motor
	pause();
  	send_byte('=');
	break;
      case 'L':  						// Instant Stop motor
	stop();
	send_byte('=');
	break;
      case 'I':						// Set Speed
	motor.speed=get_u16();
	motor.phase=SPEEDCHANGE;
	//if (GETMOTORPROPERTY(motor.status, RUNNING)) compute_timer();
	send_byte('='); 
	break;
      case 'G':	
	{					// Set Mode/Direction
	unsigned char modedir=get_u8(); 
	if (modedir&0x0F) SETMOTORPROPERTY(motor.status, BACKWARD); else UNSETMOTORPROPERTY(motor.status, BACKWARD);
	modedir=(modedir>>4); 
	if (modedir==0) { UNSETMOTORPROPERTY(motor.status, SLEWMODE); SETMOTORPROPERTY(motor.status, HIGHSPEED);}
	  else if (modedir==1) { SETMOTORPROPERTY(motor.status, SLEWMODE); UNSETMOTORPROPERTY(motor.status, HIGHSPEED); }
	  else if (modedir==2) { UNSETMOTORPROPERTY(motor.status, SLEWMODE); UNSETMOTORPROPERTY(motor.status, HIGHSPEED);}
	  else if (modedir==3) { SETMOTORPROPERTY(motor.status, SLEWMODE); SETMOTORPROPERTY(motor.status, HIGHSPEED); }
	}
	send_byte('='); 
	break;
      case 'U':                                          	// Set BreakSteps
	motor.breaks=get_u24();
	send_byte('=');
	break;
      case 'd':
	ENA=0; ENB=0;
	UNSETMOTORPROPERTY(motor.status, INITIALIZED);
	send_byte('=');
	break;
      default: goto cant_do;
      }
      goto next_cmd;
    cant_do: send_byte('!'); // Can't execute command 
    next_cmd: send_byte('\x0d'); usart_index=0,usart_rdy=0;
    } // Usart received 
  }
}
